//
// Created by fyt79 on 2019/6/9.
//
#include <stdio.h>
#include <stdlib.h>
#include <sbdsp/sbfir_special.h>
#include <sbdsp/sbfir.h>
#include "table.h"
#include "include.h"
//#include "data_sorce.h"

short i_send_buff[2048];
short q_send_buff[2048];

short i_base_buff[2048];
short q_base_buff[2048];

short out_i96x_first[32*17+16];
short out_q96x_first[32*17+16];
short out_i96x_second[1024*2+16*4];
short out_q96x_second[1024*2+16*4];
short out_i15x_third[5120*4];
short out_q15x_third[5120*4];

int thread_start = 0;
int interp_buff_select = 0;
int interp_copy_buff_select = 1;
int time_count = -1;

void transmit_init() {
    MMIO_WRITE32(SBARMSOC_MMIO_SBDC, SBDC_PSD_SHARE, 0x0F);
    set_transmit_frequency("6000");
    sbfir32Xinterpolation96tap_gmsk_coeffs(coeff96x_first, coeff96x_first_rev);
    sbfir32Xinterpolation96tap_gmsk_coeffs(coeff96x_second, coeff96x_second_rev);
    sbfirInterpolation_coeffs(coeff15x_third, coeff15x_third_rev, 5);
    start_thread(2, 0, 0, 0, transmit_thread, 0);
    start_thread(2, 1, 0, 0, produce_5120x_qdata_thread, 0);
    start_thread(2, 2, 0, 0, produce_5120x_idata_thread, 0);
    thread_start = 1;
}

void* transmit_thread() {
    int base = 0;
    int8_t buffer_select = 0;
    short* i_buffer;
    short* q_buffer;
    short mix[2048];
    short offset[16] = {0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000};
    int i;
    int time1,time2;
    stop_d2a(0);
    reset_interrupt_d2a(0);
    start_d2a(0, 2, 2048, (unsigned short*)i_send_buff, (unsigned short*)q_send_buff);
    while (1) {
        if (thread_start == 1) {
            while (is_interrupt_d2a(0) != 1);
            reset_interrupt_d2a(0);
//            time1 = __sb_cfsr(MACH_CYC);
            time_count++;
            if (time_count == 5) {
                time_count = 0;
                interp_copy_buff_select = interp_buff_select;
                interp_buff_select = (interp_buff_select + 1) % 2;
            }
            if (base >= FS / STEP) {
                base = 0;
            }
            if (buffer_select == 0) {
                buffer_select = 1;
                i_buffer = i_send_buff;
                q_buffer = q_send_buff;
            } else {
                buffer_select = 0;
                i_buffer = i_send_buff + 1024;
                q_buffer = q_send_buff + 1024;
            }
//        for (i = 0; i < 2048; i += 16, base += 16) {
//            __sb_rmuls(mix + i, cos_table + base, i_base_buff + i);
//            __sb_rmac(mix + i, sin_table + base, q_base_buff + i, mix + i);
//            __sb_radd(mix + i, mix + i, offset);
//        }
            for (i = 0; i < 2048; i += 16, base += 16) {
                __sb_rmuls(mix + i, cos_table + base, out_i15x_third + interp_copy_buff_select * 10240 + time_count * 2048 + i);
                __sb_rmacs(mix + i, sin_table + base, out_q15x_third + interp_copy_buff_select * 10240 + time_count * 2048 + i, mix + i);
                __sb_radd(mix + i, mix + i, offset);
            }
            for (i = 0; i < 1024; i += 16) {
                __sb_rshf1(i_buffer + i, q_buffer + i, mix + i * 2, mix + i * 2 + 16, 7);
            }
//        time2 = __sb_cfsr(MACH_CYC);
//        printf("clock nco used: %fus\n", (time2 - time1) / 125.0);
        }
    }
}

void* produce_5120x_idata_thread() {
    int time1,time2,i;
    int first_count = 0;
    int second_count = 0;
    int total_count = 0; //baseband data
    while (1) {
        if (thread_start == 1 && time_count == 0) {
//            time1 = __sb_cfsr(MACH_CYC);
            for (i = 0; i < 2; i++) {
                if (first_count == 0) {
                    sbfir32Xinterpolation96tap_gmsk_asm(cazac127_1i_repeat16 + total_count * 16, coeff96x_first_rev, out_i96x_first + 32, 16);// fill second 32--last 32
                    total_count++;
                }
                sbfir32Xinterpolation96tap_gmsk_asm(out_i96x_first + 16 + first_count * 32, coeff96x_second_rev, out_i96x_second + second_count * 1056, 32);// handle first 32--second last 32
                if (second_count == 0) {
                    __sb_rcopy(out_i96x_second + 2096, out_i96x_second + 16);//copy buf1 first 16 to buf2 last 16
                    second_count = 1;
                } else {
                    __sb_rcopy(out_i96x_second + 1040, out_i96x_second + 1072);//copy buf2 first 16 to buf1 last 16
                    second_count = 0;
                }
                sbfirInterpolation_asm(out_i96x_second + 16 + second_count * 1056, coeff15x_third_rev, out_i15x_third + interp_buff_select * 10240 + i * 5120, 1024, 5);
                first_count++;
                if (first_count == 16)  {
                    first_count = 0;
                    __sb_rcopy(out_i96x_first + 16, out_i96x_first + 16 + 32 * 16);//copy last 32 to first 32
                    __sb_rcopy(out_i96x_first + 32, out_i96x_first + 32 + 32 * 16);
                }
            }
//            time2 = __sb_cfsr(MACH_CYC);
//            printf("clock i_interp used: %fus\n", (time2 - time1) / 125.0);
        }
    }
}
void* produce_5120x_qdata_thread() {
    int time1,time2,i;
    int first_count = 0;
    int second_count = 0;
    int total_count = 0; //baseband data

    while (1) {
        if (thread_start == 1 && time_count == 0) {
//            time1 = __sb_cfsr(MACH_CYC);
            for (i = 0; i < 2; i++) {
                if (first_count == 0) {
                    sbfir32Xinterpolation96tap_gmsk_asm(cazac127_1q_repeat16 + total_count * 16, coeff96x_first_rev, out_q96x_first + 32, 16);// fill second 32--last 32
                    total_count++;
                }
                sbfir32Xinterpolation96tap_gmsk_asm(out_q96x_first + 16 + first_count * 32, coeff96x_second_rev, out_q96x_second + second_count * 1056, 32);// handle first 32--second last 32
                if (second_count == 0) {
                    __sb_rcopy(out_q96x_second + 2096, out_q96x_second + 16);//copy buf1 first 16 to buf2 last 16
                    second_count = 1;
                } else {
                    __sb_rcopy(out_q96x_second + 1040, out_q96x_second + 1072);//copy buf2 first 16 to buf1 last 16
                    second_count = 0;
                }
                sbfirInterpolation_asm(out_q96x_second + 16 + second_count * 1056, coeff15x_third_rev, out_q15x_third + interp_buff_select * 10240 + i * 5120, 1024, 5);
                first_count++;
                if (first_count == 16) {
                    first_count = 0;
                    __sb_rcopy(out_q96x_first + 16, out_q96x_first + 16 + 32 * 16);//copy last 32 to first 32
                    __sb_rcopy(out_q96x_first + 32, out_q96x_first + 32 + 32 * 16);
                }
            }
//            time2 = __sb_cfsr(MACH_CYC);
//            printf("clock q_interp used: %fus\n", (time2 - time1) / 125.0);
        }
    }
}
//void* transport_data_thread() {
//    int i;
//    while (1) {
//        if (base_is_busy == 0) {
//            if (time_count == 0) {
//                interp_copy_buff_select = (interp_buff_select + 1) % 2;
//            }
//            for (i = 0; i < 1024; i += 16) {
//                __sb_rcopy( i_base_buff + i, out_i15x_third + interp_copy_buff_select * 5120 + time_count * 1024 + i);
//                __sb_rcopy( q_base_buff + i, out_q15x_third + interp_copy_buff_select * 5120 + time_count * 1024 + i);
//            }
//        }
//    }
//}
void sbfirInterpolation_coeffs(short * coeff, short * coeff_rev, int phase){ //滤波器系数转换为内插函数所适用的顺序,phase为内插倍数
    int j;
    for (j = 0; j < phase; j++) {
        coeff_rev[j * 16 + 2] = coeff[j];
        coeff_rev[j * 16 + 1] = coeff[phase + j];
        coeff_rev[j * 16] = coeff[phase * 2 + j];
    }
}
void sbfirInterpolation_asm(short * in, short * coeff, short * out, int len, int phase){ //内插滤波函数，coeff为转换过的系数，len为输入长度，phase为内插倍数
    int i,j;
    short temp[1024];
//    for(i=0;i<len;i++){
//        for(j=0;j<phase;j++){
//            out[i*phase+j] = __sb_rmulreds(0,coeff+16*j,in+i)>>16;
//        }
//    }
    for (i = 0; i < phase; i++) {
        sbfir_16tap(in, coeff + 16 * i, temp, len);
        for (j = 0; j < len; j++) {
            out[phase * j + i] = temp[j];
        }
    }
}
void set_transmit_frequency(const char* frequency) {
    int frequency_num = atoi(frequency);
    int cycle;
    int i;
    double temp;
    double step;
    cycle = frequency_num / (STEP / 1000);
    step = 2 * PI * cycle / (double)(FS / STEP);
#pragma doall 4
    for (i = 0; i < FS / STEP; i++) {
        temp = sin(step * i) + 1;
        sin_table[i] = (temp * 32767.5) - 32768;
        temp = cos(step * i) + 1;
        cos_table[i] = (temp * 32767.5) - 32768;
    }
    printf("ok freq=%d\n", cycle * (STEP / 1000));
}
/*
void* transmit_thread() {
  int i;
  //for (i = 0; i < 8192; i++) {
  //  if (i % 2 == 0) {
  //    i_send_buff[i] = 0;
  //    q_send_buff[i] = 0;
  //  } else {
  //     i_send_buff[i] = 65535;
  //     q_send_buff[i] = 65535;
  //  }
  //}
  set_transmit_frequency("12000");
    {
        int i;
    }
  for (i = 0; i < FS / STEP / 2; i++) {
    i_send_buff[i] = sin_table[i * 2] << 8;
    q_send_buff[i] = sin_table[i * 2 + 1] << 8;
  }
  MMIO_WRITE32(SBARMSOC_MMIO_SBDC, SBDC_PSD_SHARE, 0x0F);
  stop_d2a(0);
  reset_interrupt_d2a(0);
  start_d2a(0, 1, FS / STEP, i_send_buff, q_send_buff);
  while (1) {
    while (is_interrupt_d2a(0) != 1);
    for (i = 0; i < FS / STEP; i++) {
      //i_send_buff[i] = sin_table[i] << 8;
      //i_send_buff[i] = sin_table[i * 2] << 8;
      //q_send_buff[i] = sin_table[i * 2 + 1] << 8;
      //q_send_buff[i] = 0;
      i_send_buff[i] = 0;
      q_send_buff[i] = sin_table[i] << 8;
    }
    reset_interrupt_d2a(0);
  }
}*/
/*
void transmit_source() {
  int currentBuffer = 0;
  int base = 0;
  MMIO_WRITE32(SBARMSOC_MMIO_SBDC, SBDC_PSD_SHARE, 0x0F);
  stop_d2a(0);
  reset_interrupt_d2a(0);
  start_d2a(0, 2, 2048, i_send_buff, q_send_buff);
  while (1) {
    while (is_interrupt_d2a(0) != 1);
    reset_interrupt_d2a(0);
    if (base >= 1536000) {
      base = 0;
    }
    if (currentBuffer == 0) {
      memcpy(i_send_buff, sin_table + base, 2048);
      memcpy(q_send_buff, cos_table + base, 2048);
      currentBuffer = 1;
    } else {
      memcpy(i_send_buff + 1024, sin_table + base, 2048);
      memcpy(q_send_buff + 1024, cos_table + base, 2048);
      currentBuffer = 0;
    }
    base += 1024;
  }
}*/

//void transmit(unsigned short* i_data, unsigned short* q_data, unsigned int length, int times) {
//  unsigned short* source_i_data_ptr = i_data;
//  unsigned short* source_q_data_ptr = q_data;
//  stop_d2a(0);
//  reset_interrupt_d2a(0);
//  start_d2a(0, 2, FS / STEP, i_send_buff, q_send_buff);
//  while (1) {
//    while (is_interrupt_d2a(0) != 1);
//    reset_interrupt_d2a(0);
//    if (current_buffer == 0) {
//      current_buffer = 1;
//      data_index = 0;
//      source_data_index = 0;
//      while(data_index < block_length) {
//        for (i = 0; i < time; i++) {
//          temp_buffer_i[data_index] = i_data[source_data_index];
//          temp_buffer_q[data_index] = q_data[source_data_index];
//          data_index++;
//        }
//        source_data_index++;
//      }
//      for (data_index = 0; data_index < block_length; data_index += 16) {
//        __sb_rmul(temp_buffer_i + data_index, i_data + data_index, sin_table + data_index);
//        __sb_rmac(temp_buffer_add + data_index, q_data[i] + data_index, cos_table + data_index, temp_buffer_i + data_index);
//      }
//    } else {
//      current_buffer = 0;
//    }
//  }
//}

//void *tx_thread(void *arg)
//{
//  MMIO_WRITE32(SBARMSOC_MMIO_SBDC, SBDC_PSD_SHARE, 0x0F);
//  stop_d2a(0);
//  while(1)
//  {
//    reset_interrupt_d2a(0);
//    printf("%d\n", psdo_ibuf[0]);
//    start_d2a(0, PSDO_BUF_COUNT, PSDO_BUF_SIZE, psdo_ibuf, psdo_qbuf);
//    while(is_interrupt_d2a(0) != 1);
//    printf("%d\n", psdo_ibuf[0]);
//    while(1)
//    {
//      while(is_interrupt_d2a(0) != 1);
//      reset_interrupt_d2a(0);
//    }
//    //else
//  }//while(1)
//}
